#ifndef ROBORTS_COSTMAP_PUNISH_LAYER_H
#define ROBORTS_COSTMAP_PUNISH_LAYER_H

#include <ros/ros.h>
#include "layer.h"
#include "layered_costmap.h"
#include "costmap_layer.h"
#include "costmap_2d.h"
#include  <roborts_costmap/Nlayer.h>

namespace roborts_costmap
{
 
class PunishLayer : public Layer, public Costmap2D
{
public:
  PunishLayer(std::string map_name);

  void  NlayerInfoCallback(const roborts_costmap:: Nlayer :: ConstPtr&msg);
  void OnInitialize();
  void UpdateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,double* max_y);
  void UpdateCosts(roborts_costmap::Costmap2D& master_new, int min_i, int min_j, int max_i, int max_j);
  bool isDiscretized()
  {
    return true;
  }
 
void MatchSize();
private:
  ros::Subscriber nlayer_sub_;
  geometry_msgs::PointStamped punish_area_1_map;
  geometry_msgs::PointStamped punish_area_2_map;
  geometry_msgs::PointStamped punish_area_3_map;
  geometry_msgs::PointStamped punish_area_4_map;
  geometry_msgs::PointStamped punish_area_5_map;
  geometry_msgs::PointStamped punish_area_6_map;
  geometry_msgs::PointStamped punish_area_7_map;
  geometry_msgs::PointStamped punish_area_8_map;
  geometry_msgs::PointStamped punish_area_9_map;
  geometry_msgs::PointStamped punish_area_1;
  geometry_msgs::PointStamped punish_area_2;
  geometry_msgs::PointStamped punish_area_3;
  geometry_msgs::PointStamped punish_area_4;
  geometry_msgs::PointStamped punish_area_5;
  geometry_msgs::PointStamped punish_area_6;
  geometry_msgs::PointStamped punish_area_7;
  geometry_msgs::PointStamped punish_area_8;
  geometry_msgs::PointStamped punish_area_9;
  std::vector<std::pair<unsigned int,unsigned int>> punish_cells;
  std::string map_name_;
  unsigned  flag;
  bool Derail1=false;
  bool Derail2=false;
  bool Derail3=false;
  bool Derail4=false;
  bool Derail5=false;
  bool Derail6=false;
  bool Derail7=true;
  bool Derail8=true;
  bool Derail9=true;
};
}
#endif